//This program is only for testing multi-priority requests for one intersetion.
//This program only simulates a priority requests and broadcasts it to inform rsu_ntcip.
//Written by Qing He 08/07/2009
#include "RNDF.h"

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <unistd.h>
#include <arpa/inet.h>

#include "gps.h"
#include <unistd.h>

#include <iostream>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fstream>
#include <sstream>
#include <istream>
#include "math.h"
#include "time.h"

#include <iomanip> 

#include <stddef.h>
#include <dirent.h>

//socket settings
#define GIDPORT 5799
#define PMTPORT 15020
//#define OBU_ADDR "192.168.1.25"
#define BROADCAST_ADDR "192.168.255.255"

char vehicleid [64];//authentic HQ 06/24/09
float laneheading; //current vehicle approaching direction
float m_vehtime;
float m_vehx;
float m_vehy;
float m_vehlat;
float m_vehlong;
float m_vehspeed;//HQ 06/24/09
int m_vehtraveltime;//estimated travel time to the approaching signal
int m_vehphase;

//define log file name
char logfilename[256] = "/usr/local/bin/obu_ntcip_log_";
char rndf_file[128]="/usr/local/bin/RNDF.txt";

double GPSdistance(double lat1, double long1, double lat2, double long2);
int outputlog(char *output);

double GPSdistance(double lat1, double long1, double lat2, double long2)
    {
    //return distance(meters) between two GPS points
    double dist_m;
    double phi1,theta1,phi2,theta2;
    double x1,y1,z1,x2,y2,z2;

    phi1 = lat1>0? 90-lat1:90+lat1;
    theta1 = long1>0?long1:long1*(-1);

    phi1 = phi1*M_PI/180.0;
    theta1 = theta1*M_PI/180.0;

    phi2 = lat2>0? 90-lat2:90+lat2;
    theta2 = long2>0?long2:long2*(-1);

    phi2 = phi2*M_PI/180.0;
    theta2 = theta2*M_PI/180.0;

    x1 = 6367000*cos(theta1)*sin(phi1);
    y1 = 6367000*sin(theta1)*sin(phi1);
    z1 = 6367000*cos(phi1);

    x2 = 6367000*cos(theta2)*sin(phi2);
    y2 = 6367000*sin(theta2)*sin(phi2);
    z2 = 6367000*cos(phi2);

    //std::cout << "Phi1="<<phi1<<"theta1"<<theta1<<std::endl;
    //std::cout << "Phi2="<<phi1<<"theta2"<<theta2<<std::endl;

    dist_m = sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)+(z1-z2)*(z1-z2));

    return dist_m;

    }
void get_veh_id() //HQ add 06/24/09
{
	std::ifstream load;
	load.open("/usr/local/bin/vehicleid.txt");
	load.getline(vehicleid,64);
	std::cout<< "Current Vehicle ID:" << vehicleid <<std::endl;
	outputlog("Current Vehicle ID:");
	outputlog(vehicleid);
}

int msleep(unsigned long milisec)
{
	struct timespec req={0};
	time_t sec=(int)(milisec/1000);
	milisec=milisec-(sec*1000);
	req.tv_sec=sec;
	req.tv_nsec=milisec*1000000L;
	while(nanosleep(&req,&req)==-1)
		continue;
	return 1;
}

//------------Time stamp------------------------------------------------
void     xTimeStamp( char * pc_TimeStamp_ )
{
          struct tm  * ps_Time;
          time_t       i_CurrentTime;
          char         ac_TmpStr[256];

          i_CurrentTime =  time(NULL);
          ps_Time = localtime( &i_CurrentTime );

                      //year
          sprintf(ac_TmpStr, "%d", ps_Time->tm_year + 1900);
          strcpy(pc_TimeStamp_, ac_TmpStr);

                      //month
          sprintf(ac_TmpStr, "_%d", ps_Time->tm_mon + 1 );
          strcat(pc_TimeStamp_, ac_TmpStr);

                     //day
          sprintf(ac_TmpStr, "_%d", ps_Time->tm_mday );
          strcat(pc_TimeStamp_, ac_TmpStr);

                     //hour
          sprintf(ac_TmpStr, "_%d", ps_Time->tm_hour  );
          strcat(pc_TimeStamp_, ac_TmpStr);

                     //min
          sprintf(ac_TmpStr, "_%d", ps_Time->tm_min );
          strcat(pc_TimeStamp_, ac_TmpStr);

                     //sec
          sprintf(ac_TmpStr, "_%d", ps_Time->tm_sec );
          strcat(pc_TimeStamp_, ac_TmpStr);
}


//end-------------------------------------------------------------------
int outputlog(char *output)
{
	FILE * stream = fopen( logfilename, "r" );
        fseek( stream, 0L, SEEK_END );
        long endPos = ftell( stream );
        fclose( stream );

	std::fstream fs;
        if (endPos <10000000)
		fs.open(logfilename, std::ios::out | std::ios::app);
	else
		fs.open(logfilename, std::ios::out | std::ios::trunc);

	//std::fstream fs;
	//fs.open("/usr/local/bin/OBU_logfile.txt", std::ios::out | std::ios::app);
	if (!fs || !fs.good())
	{
		std::cout << "could not open file!\n";
		return -1;
	}
	fs << output << std::endl;
	if (fs.fail())
	{
		std::cout << "failed to append to file!\n";
		return -1;
	}
	return 1;
}


static struct gps_data_t       *gpsdata;
static char gps_sqlnames[64], gps_sqlvals[128];

#define NOW(__gps_data)  ((isnan(__gps_data->fix.time)==0) ? \
    (time_t)__gps_data->fix.time : time(NULL))


int gps_init()
    {
    gpsdata = gps_open("127.0.0.1", "2947");
    if (!gpsdata) {
        printf("Error: Failed to connect to gps\n");
        return -1;
        }
    gps_query(gpsdata, "o\n"); /* update fix once */
    usleep(1000*1000); /* give it sometime to get first fix */
    return 0;
    }

int main ( int argc, char* argv[] )
{
	//Init log file

	char aa[256];
	char bb[256];
	char pBuf[256];
    //char strbuf[256];    
	char sendMSG[256];
	char sz_temp[256];
	//------log file name with Time stamp---------------------------
	char timestamp[128];
	xTimeStamp(timestamp);
	strcat(logfilename,timestamp);
	strcat(logfilename,".log");
	//------end log file name-----------------------------------

    //--------Start of GPS part---------
    if ( gps_init() ) return -1;

    printf("About to query gps data....\n");
    //--------End of GPS part---------

	std::fstream fs;
	fs.open(logfilename, std::ios::out | std::ios::trunc );
	fs << "obu tester for multi-priorty control is running...." << std::endl;

	//------Get obu id-------------------------------------------
	get_veh_id();

//--------------------------init----------------------------------------------------
	//init socket
		    int sockfd;

	        struct sockaddr_in sendaddr;
        	struct sockaddr_in recvaddr;
	        int numbytes;
        	int addr_len;
        	int broadcast=1;
	 //----------------------Preemption test , directly send preemption to broadcast address, port 15020-------

		if((sockfd = socket(PF_INET,SOCK_DGRAM,0)) == -1)
	        {
        	        perror("sockfd");
        	        exit(1);
        	}

       		if((setsockopt(sockfd,SOL_SOCKET,SO_BROADCAST,
                                &broadcast,sizeof broadcast)) == -1)
        	{
        	        perror("setsockopt - SO_SOCKET ");
        	        exit(1);
        	}

	        sendaddr.sin_family = AF_INET;
        	sendaddr.sin_port = htons(PMTPORT);
        	sendaddr.sin_addr.s_addr = INADDR_ANY;//inet_addr(OBU_ADDR);//INADDR_ANY;
        	memset(sendaddr.sin_zero,'\0',sizeof sendaddr.sin_zero);

        	if(bind(sockfd, (struct sockaddr*) &sendaddr, sizeof sendaddr) == -1)
        	{
        	        perror("bind");
        	        exit(1);
        	}

		  // HQ Change
	        recvaddr.sin_family = AF_INET;
	        recvaddr.sin_port = htons(PMTPORT);
	        recvaddr.sin_addr.s_addr = inet_addr(BROADCAST_ADDR) ; //INADDR_BROADCAST;
        	memset(recvaddr.sin_zero,'\0',sizeof recvaddr.sin_zero);

		int addr_length = sizeof ( recvaddr );

//dead loop to send message
		m_vehtraveltime = 30;
		m_vehphase = rand()%8+1;
        int OK=0;
		while ( true )
		{
            OK++;
            if(OK==10) break;
			outputlog("  ");
			msleep(3000);
//----------------------------Read all the rndf file in the directory /usr/local/bin---------

            /*
            *** The RNDF.txt file includes at most two rows.
            *** The last row will be the RNDF I am at now.
            *** 
            */
            fstream fss;
            fss.open(rndf_file);//rndf_file[128]="/usr/local/bin/RNDF.txt";
            string lineread;
            char rndfname[128];
            int attime;            
            if(fss.good())
                {
                while(!fss.eof())
                    {                              
                    getline(fss,lineread);
                    if(lineread.size()!=0)
                        {
                        if(sscanf(lineread.c_str(),"%s %d",rndfname,&attime)!=2)
                            {
                            perror("Not all fields are assigned.");
                            outputlog("Not all fields are assigned when read RNDF.txt.");
                            exit(1);
                            }                                  
                        }
                    }
                }
            else
                {
                perror("cannot open RNDF.txt file.");
                outputlog("cannot open RNDF.txt file.");
                exit(1);
                }

            fss.close();
            string rndf_file_name=rndfname;
            if(rndf_file_name.size()<2) // if NOT on any GID.
                {
                outputlog("Do not have any GID!");
                exit(1); // OR use "continue"
                }
            else
                {
                RNDF rndf=RNDF(rndf_file_name,false);

                map<ElementID, LatLong, id_comparator>::iterator id_ll_mapi;
                map<ElementID, LatLong, id_comparator>::iterator exit_mapi;
                //---- Start Getting the GPS position------------------
                gps_query(gpsdata, "w+x");

                if (isnan(gpsdata->fix.time)==0)
                    {
                    time_t  now = (time_t)gpsdata->fix.time;
                    char    buf[64];
                    ctime_r(&now, buf);
                    buf[strlen(buf)-1] = 0; /* take out last newline char */
                    printf("%s:: ", buf);
                    }

                if (gpsdata->fix.mode >= MODE_2D && isnan(gpsdata->fix.latitude)==0)
                    {
                    printf("Lat: %.6f %c, ", fabs(gpsdata->fix.latitude),
                        (gpsdata->fix.latitude < 0) ? 'S' : 'N');
                    }
                if (gpsdata->fix.mode >= MODE_2D && isnan(gpsdata->fix.longitude)==0)
                    {
                    printf("Long: %.6f %c, ", fabs(gpsdata->fix.longitude),
                        (gpsdata->fix.longitude < 0) ? 'W' : 'E');
                    }
                if (gpsdata->fix.mode == MODE_3D && isnan(gpsdata->fix.altitude)==0)
                    {
                    printf("Alt: %.1f m\n", gpsdata->fix.altitude);
                    }

                //---- End of Getting the GPS position-----------------
                
                cout<<"Current GPS position:\t"<<gpsdata->fix.latitude<<"  "<<gpsdata->fix.longitude<<endl;
                //LatLong LL_test=LatLong(gpsdata->fix.altitude,gpsdata->fix.longitude);
                LatLong LL_test=LatLong(32.235931,-110.955378);
                double speed=gpsdata->fix.speed;  // gps_fix_t: m/s

                double ShortDis=10000.0;
                cout<<endl<<endl<<endl;
                cout<<"-----------------------------------------"<<endl;
                ElementID DesID;

                for(id_ll_mapi=rndf.id_ll_map.begin();id_ll_mapi!=rndf.id_ll_map.end();id_ll_mapi++)
                    {
                    LatLong LL_temp=id_ll_mapi->second; 

                    double temp=GPSdistance(LL_test.latitude,LL_test.longitude,LL_temp.latitude,LL_temp.longitude);

                    if(temp<ShortDis)
                        {
                        ShortDis=temp;
                        cout<<"TEMP distance is:"<<temp<<endl;
                        DesID=id_ll_mapi->first;            
                        }
                    }

                cout<<"ID is :SEG "<<DesID.seg<<" LANE "<<DesID.lane<<" PT "<<DesID.pt<<endl;

                ElementID DesID1=DesID;
                DesID1.pt=1;
                int phase=rndf.phaseNo_map[DesID1]; // will change the PT to 1.---important.

                for(exit_mapi=rndf.exit_map.begin();exit_mapi!=rndf.exit_map.end();exit_mapi++)
                    {
                    if(DesID.seg==exit_mapi->first.seg && DesID.lane==exit_mapi->first.lane)
                        DesID.pt=exit_mapi->first.pt;
                    }
                LatLong DesLL=rndf.exit_map[DesID];

                cout<<"Exit LL:"<<DesLL.latitude<<"  "<<DesLL.longitude<<endl;

                double dis=GPSdistance(LL_test.latitude,LL_test.longitude,DesLL.latitude,DesLL.longitude);
                cout<<"Phase is:"<<phase<<"   Distance is: "<<dis<<endl;
                
                m_vehtraveltime=int(dis/10.0);  // Speed is about 10.0 m/s;
                //*** Generate the Priority Request--------
                char strbuf[128]="";    
                sprintf(strbuf,"%s 1 %s %d %d",rndf_file_name.c_str(),vehicleid,phase,m_vehtraveltime);//vehicle class:1 phase:random from 1-8
                //strcat(sendMSG,strbuf);
                strcpy(sendMSG,strbuf);

                sprintf(sz_temp,"Send message: %s",sendMSG);
                std::cout <<sz_temp<<std::endl;
                outputlog(sz_temp);

                numbytes = sendto(sockfd, sendMSG,strlen(sendMSG)+1 , 0,(struct sockaddr *)&recvaddr, addr_length);
                if (numbytes>0)
                    {
                    sprintf(sz_temp,"Send message successfully..");
                    std::cout <<sz_temp<<std::endl;
                    outputlog(sz_temp);
                    }
                }
		}
        return 0;

}